import math
import numpy as np
from Node import Node
from point import point
from plotmaze import plotSparse
from plotmaze import plotAnimation


class Astar:  # 定义A*算法中的函数
    def __init__(self, obstacle, startnode, endnode, length, width, maxl, minl, maxangle):
        self.openList = []
        self.closeList = []  # 维护的open与close表

        self.mazeObstacle = obstacle  # 地图信息，记录不可行点
        self.startNode = startnode
        self.endNode = endnode
        self.currNode = startnode  # 全局起始终点节点信息，以及更新的当前节点
        self.path = []  # 记录路径

        self.length = length
        self.width = width

        self.danger = 0  # 危险指数，航迹段到所有障碍物中心距离总和
        self.maxL = maxl
        self.minL = minl
        self.step = 0
        self.theta = 0
        self.maxAngle = maxangle

    def minFvalue(self):  # 计算openlist中的最小f节点
        if len(self.openList) == 0:
            listvoid = []
            plotSparse(self.mazeObstacle, listvoid, (self.startNode.point.x, self.startNode.point.y),
                       (self.endNode.point.x, self.endNode.point.y), self.length, self.width)
            # 原设计是openlist为空时，绘制地图，但是我没有在未找到路径时清空list，所以好像大多数时候不起效
            return None
        else:
            minNode = self.openList[0]
            for tempNode in self.openList:  # 取最小f对应的结点
                if tempNode.f < minNode.f:
                    minNode = tempNode
            return minNode

    def isOpen(self, thisNode):  # 判断该节点是否在openlist中
        for tempNode in self.openList:
            if tempNode.point.x == thisNode.point.x and tempNode.point.y == thisNode.point.y:
                return True
        return False

    def isclose(self, thisNode):  # 判断该节点是否在closelist中
        for tempNode in self.closeList:
            if tempNode.point.x == thisNode.point.x and tempNode.point.y == thisNode.point.y:
                return True
        return False

    def isLaststep(self):  # 判断openlist中是否含有end点，判断是否为最后一步
        for tempNode in self.openList:
            if pow(tempNode.point.x - self.endNode.point.x, 2) + pow(tempNode.point.y - self.endNode.point.y, 2) <= pow(
                    self.maxL, 2):
                return True
        return False

    def Laststep(self):  # 判断openlist中是否含有end点，判断是否为最后一步
        for tempNode in self.openList:
            if pow(tempNode.point.x - self.endNode.point.x, 2) + pow(tempNode.point.y - self.endNode.point.y, 2) <= pow(
                    self.maxL, 2):
                return tempNode

    def getNodeinOL(self, thisNode):  # 通过坐标索引open表中的节点和信息
        for tempNode in self.openList:
            if thisNode.point.x == tempNode.point.x and thisNode.point.y == tempNode.point.y:
                return tempNode
        return None

    def isAccess(self, thisPoint):  # 判断point是否可通行
        if thisPoint.x < 0 or thisPoint.y < 0:  # 低于下界
            return False
        if thisPoint.x >= self.length - 1 or thisPoint.y >= self.width - 1:  # 高于上界
            return False
        for i in self.mazeObstacle:  # 处于障碍物内
            if pow(i[0] - thisPoint.x, 2) + pow(i[1] - thisPoint.y, 2) <= pow(i[2], 2):
                return False
        return True

    def Heron(self, a, b, c):
        p = (a + b + c) / 2
        S = math.sqrt(p * (p - a) * (p - b) * (p - c))
        h = 2 * S / a
        return h

    def searchNode(self, thisNode):
        if not self.isAccess(thisNode.point):  # 不可行点
            return
        if self.isclose(thisNode):  # 已经遍历过该点
            return

        tempx, tempy = [thisNode.point.x, thisNode.point.y]
        currx, curry = [self.currNode.point.x, self.currNode.point.y]  # 操作坐标，进行探索
        D = 0  # 总距离
        mark = 0
        for i in self.mazeObstacle:
            newDx = tempx - i[0]
            newDy = tempy - i[1]
            nearDx = currx - i[0]
            nearDy = curry - i[1]
            newO = math.sqrt(pow(newDx, 2) + pow(newDy, 2))
            nearO = math.sqrt(pow(nearDx, 2) + pow(nearDy, 2))
            newNear = math.sqrt(pow(newDx - nearDx, 2) + pow(newDy - nearDy, 2))
            h = self.Heron(newNear, newO, nearO)  # 海伦公式
            cosnew = (pow(newNear, 2) + pow(newO, 2) - pow(nearO, 2)) / (2 * newO * newNear)
            cosnear = (pow(newNear, 2) + pow(nearO, 2) - pow(newO, 2)) / (2 * nearO * newNear) # 余弦定理
            if h <= i[2] and cosnew > 0 and cosnear > 0:
                mark = 0
                break
            else:
                d = (abs((curry - tempy) * i[0] - (currx - tempx) * i[1] + tempy * (currx - tempx) - tempx * (
                        curry - tempy))) / \
                    (math.sqrt(pow(curry - tempy, 2) + pow(currx - tempx, 2)))  # 圆心到连线线段的距离，O为圆心
                D = D + d
                mark = 1

        if mark == 0:  # 凡mark变为0，则路径不可行
            return

        if abs(tempx - currx) > 1:  # 更新gValue，根据之前选的是大步长还是小步长，将总距离的分数作为威胁度
            gValue = self.maxL + 1 / D
        else:
            gValue = self.minL + 1 / D

        if not self.isOpen(thisNode):  # 不在open表中，则进行操作
            thisNode.getGvalue(gValue + self.currNode.g)  # 更新该节点g
            thisNode.getManhattendis(self.endNode)  # 更新该节点h
            thisNode.ancient = self.currNode  # 更新该节点父亲节点
            self.openList.append(thisNode)  # 加入open表中
        else:  # 若已经在open表中
            tempNode = self.getNodeinOL(thisNode)  # 把open表中的该点找出来
            if self.currNode.g + gValue < tempNode.g:  # 更新g、以及父亲节点
                tempNode.g = self.currNode.g + gValue
                tempNode.ancient = self.currNode
        return

    def searchWay(self):
        self.step = self.maxL  # 默认大步长进行前进
        childrenNum = 0  # 设置子数目，每一个方向可行则子节点数目加一，并进行探索节点
        if self.isAccess(point(round(self.currNode.point.x + math.cos((self.theta + 0) * math.pi / 180) * self.step),
                               round(self.currNode.point.y + math.sin((self.theta + 0) * math.pi / 180) * self.step))):
            childrenNum = childrenNum + 1
            self.searchNode(
                Node(point(round(self.currNode.point.x + math.cos((self.theta + 0) * math.pi / 180) * self.step),
                           round(self.currNode.point.y + math.sin((self.theta + 0) * math.pi / 180) * self.step))))
        if self.isAccess(point(round(
                self.currNode.point.x + math.cos((self.theta + self.maxAngle / 2) * math.pi / 180) * self.step),
                round(self.currNode.point.y + math.sin(
                    (self.theta + self.maxAngle / 2) * math.pi / 180) * self.step))):
            childrenNum = childrenNum + 1
            self.searchNode(Node(point(
                round(self.currNode.point.x + math.cos((self.theta + self.maxAngle / 2) * math.pi / 180) * self.step),
                round(self.currNode.point.y + math.sin((self.theta + self.maxAngle / 2) * math.pi / 180) * self.step))))
        if self.isAccess(
                point(round(self.currNode.point.x + math.cos((self.theta + self.maxAngle) * math.pi / 180) * self.step),
                      round(
                          self.currNode.point.y + math.sin((self.theta + self.maxAngle) * math.pi / 180) * self.step))):
            childrenNum = childrenNum + 1
            self.searchNode(Node(
                point(round(self.currNode.point.x + math.cos((self.theta + self.maxAngle) * math.pi / 180) * self.step),
                      round(
                          self.currNode.point.y + math.sin((self.theta + self.maxAngle) * math.pi / 180) * self.step))))
        if self.isAccess(point(round(
                self.currNode.point.x + math.cos((self.theta - self.maxAngle / 2) * math.pi / 180) * self.step),
                round(self.currNode.point.y + math.sin(
                    (self.theta - self.maxAngle / 2) * math.pi / 180) * self.step))):
            childrenNum = childrenNum + 1
            self.searchNode(Node(point(
                round(self.currNode.point.x + math.cos((self.theta - self.maxAngle / 2) * math.pi / 180) * self.step),
                round(self.currNode.point.y + math.sin((self.theta - self.maxAngle / 2) * math.pi / 180) * self.step))))
        if self.isAccess(
                point(round(self.currNode.point.x + math.cos((self.theta - self.maxAngle) * math.pi / 180) * self.step),
                      round(
                          self.currNode.point.y + math.sin((self.theta - self.maxAngle) * math.pi / 180) * self.step))):
            childrenNum = childrenNum + 1
            self.searchNode(Node(
                point(round(self.currNode.point.x + math.cos((self.theta - self.maxAngle) * math.pi / 180) * self.step),
                      round(
                          self.currNode.point.y + math.sin((self.theta - self.maxAngle) * math.pi / 180) * self.step))))

        if childrenNum <= 2:  # 子节点数量过少，说明障碍物过于密集，使用小步长
            self.step = self.minL  # 将大步长转化为小步长
            childrenNum = 0
            # 以小步长进行探索，成功则和之前的大步长成果的结果（数量小于等于2）一起探索
            if self.isAccess(
                    point(round(self.currNode.point.x + math.cos((self.theta + 0) * math.pi / 180) * self.step),
                          round(self.currNode.point.y + math.sin((self.theta + 0) * math.pi / 180) * self.step))):
                self.searchNode(
                    Node(point(round(self.currNode.point.x + math.cos((self.theta + 0) * math.pi / 180) * self.step),
                               round(self.currNode.point.y + math.sin((self.theta + 0) * math.pi / 180) * self.step))))
            if self.isAccess(point(round(
                    self.currNode.point.x + math.cos((self.theta + self.maxAngle / 2) * math.pi / 180) * self.step),
                    round(self.currNode.point.y + math.sin(
                        (self.theta + self.maxAngle / 2) * math.pi / 180) * self.step))):
                self.searchNode(Node(point(
                    round(
                        self.currNode.point.x + math.cos((self.theta + self.maxAngle / 2) * math.pi / 180) * self.step),
                    round(self.currNode.point.y + math.sin(
                        (self.theta + self.maxAngle / 2) * math.pi / 180) * self.step))))
            if self.isAccess(
                    point(round(
                        self.currNode.point.x + math.cos((self.theta + self.maxAngle) * math.pi / 180) * self.step),
                        round(self.currNode.point.y + math.sin(
                            (self.theta + self.maxAngle) * math.pi / 180) * self.step))):
                self.searchNode(Node(
                    point(round(
                        self.currNode.point.x + math.cos((self.theta + self.maxAngle) * math.pi / 180) * self.step),
                        round(self.currNode.point.y + math.sin(
                            (self.theta + self.maxAngle) * math.pi / 180) * self.step))))
            if self.isAccess(point(round(
                    self.currNode.point.x + math.cos((self.theta - self.maxAngle / 2) * math.pi / 180) * self.step),
                    round(self.currNode.point.y + math.sin(
                        (self.theta - self.maxAngle / 2) * math.pi / 180) * self.step))):
                self.searchNode(Node(point(
                    round(
                        self.currNode.point.x + math.cos((self.theta - self.maxAngle / 2) * math.pi / 180) * self.step),
                    round(self.currNode.point.y + math.sin(
                        (self.theta - self.maxAngle / 2) * math.pi / 180) * self.step))))
            if self.isAccess(
                    point(round(
                        self.currNode.point.x + math.cos((self.theta - self.maxAngle) * math.pi / 180) * self.step),
                        round(self.currNode.point.y + math.sin(
                            (self.theta - self.maxAngle) * math.pi / 180) * self.step))):
                self.searchNode(Node(point(
                    round(self.currNode.point.x + math.cos((self.theta - self.maxAngle) * math.pi / 180) * self.step),
                    round(self.currNode.point.y + math.sin((self.theta - self.maxAngle) * math.pi / 180) * self.step))))

        return

    def run(self):
        self.startNode.getManhattendis(self.endNode)
        self.startNode.getGvalue(0)  # 初始化起点的g与h以及f

        self.openList.append(self.startNode)
        self.currNode = self.minFvalue()  # 将初始点加入open表中，并作为currNode
        if self.currNode is None:
            print("生成无结果")  # 以防万一，其实没遇到过
            return

        if self.currNode.ancient is None:  # 初始点
            a = self.endNode.point.x - self.currNode.point.x
            b = self.endNode.point.y - self.currNode.point.y
            thetaTemp = abs(math.acos(a / np.linalg.norm([a, b], 2)) * 180 / math.pi)  # 终点与该点关于横轴的夹角，用角度制表示
            if (a >= 0 and b >= 0) or (a <= 0 <= b):  # 让角度有负有正，一二象限用正数表示，三四象限用负数表示，区分不同位置但相同角度的情况
                self.theta = thetaTemp
            elif (a >= 0 >= b) or (a <= 0 and b <= 0):
                self.theta = -thetaTemp
        else:  # 如果不是初始点（也是为了安全起见，实则不会出现）
            a = self.currNode.point.x - self.currNode.ancient.point.x  # 和父亲节点的横轴距离
            b = self.currNode.point.y - self.currNode.ancient.point.y  # 和父亲节点的纵轴距离
            if abs(a) > 1:
                self.step = self.maxL
            if abs(a - self.step) < 0.01:
                a = self.step
            print("????")
            thetaTemp = abs(math.acos(a / self.step) * 180 / math.pi)  # 步长作为直角的对边，计算角度
            if (a >= 0 and b >= 0) or (a <= 0 <= b):  # 让角度有负有正，一二象限用正数表示，三四象限用负数表示，区分不同位置但相同角度的情况
                self.theta = thetaTemp
            elif (a >= 0 >= b) or (a <= 0 and b <= 0):
                self.theta = -thetaTemp
        # 至此求得基础角
        self.closeList.append(self.currNode)  # 将该节点放入close表中
        pathlisttest = []
        self.searchWay()  # 探索周围节点，这里需要step的更新，不然后面step为0不能除
        self.openList.remove(self.currNode)  # 探索完毕，从open中删除该节点
        while True:  # 开始遍历
            self.currNode = self.minFvalue()  # 从open表中选currNode进行操作
            if self.currNode.ancient is not None:
                a = self.currNode.point.x - self.currNode.ancient.point.x  # 和父亲节点的横轴距离
                b = self.currNode.point.y - self.currNode.ancient.point.y  # 和父亲节点的纵轴距离
                if abs(a) > 1:
                    self.step = self.maxL
                else:
                    self.step = self.minL

                thetaTemp = abs(math.acos(round(a / self.step, 10)) * 180 / math.pi)  # 步长作为直角的对边，计算角度
                if (a >= 0 and b >= 0) or (a <= 0 <= b):  # 让角度有负有正，一二象限用正数表示，三四象限用负数表示，区分不同位置但相同角度的情况
                    self.theta = thetaTemp
                elif (a >= 0 >= b) or (a <= 0 and b <= 0):
                    self.theta = -thetaTemp

            self.closeList.append(self.currNode)
            self.searchWay()
            self.openList.remove(self.currNode)
            pathlisttest.append((self.currNode.point.x + 0.5, self.currNode.point.y + 0.5))
            # plotAnimation(self.mazeObstacle, self.closeList, self.currNode.point, self.currNode.ancient.point,
            # self.startNode.point, self.endNode.point, self.length, self.width)

            # 上面注释取消后，可逐步显示寻路过程

            if self.isLaststep():  # 下一步可以到达终点(在open表中说明已经建立父节点关系)
                tempNode = self.endNode
                tempNode.point.x = tempNode.point.x - 1
                tempNode.point.y = tempNode.point.y - 1
                tempNode.ancient = self.Laststep()
                while True:
                    self.path.append(tempNode)
                    if tempNode.ancient is not None:
                        tempNode = tempNode.ancient
                    else:
                        # plotAnimation(self.mazeObstacle, self.closeList, self.currNode.point,
                        #               self.currNode.ancient.point, self.startNode.point, self.endNode.point,
                        #               self.length, self.width)
                        return self.path
            elif len(self.openList) == 0:
                return False
